package roborally;
import Exceptions.IllegalNumberException;
import Exceptions.IllegalPositionException;
/**
 * class that implements a robot that has a specific position, orientation and
 * amount of energy
 * 
 * @invar the energy the robot has must be a positive amount and less than then maximum energy
 * 		| isValidEnergyAmount(getEnergy)
 * @author arnau & wim
 * @version 1.0
 */
public class Robot implements IRobot {
	/**
	 * makes a new robot with given energy,position at the given coordinates and with the given orientation
	 * 
	 * @param energy
	 *            amount of energy the robot has left
	 * @param position
	 *            the position of the robot on the board
	 * @param orientation
	 *            the orientation of the robot
	 * @post the new robot Energy is set to the given energy amount
	 * 		| new getEnergy() == energy
	 * @post the new robot starting position is equals to the given starting position
	 * 		| new getPosition() == position
	 * @post the new robot starts with a given orientation 
	 * 		| new getOrientation() == orientation
	 * @throws IllegalPositionException(position.this) t
	 * 			he new robot can not have this as starting position 
	 * 		| !isValidPosition(position)
	 */
	public Robot(double energy, long[] position, int orientation) throws IllegalPositionException {
		setEnergy(energy);
		setPosition(position);
		setOrientation(orientation);
	}
	/**
	 * Returns the current energy the robot has
	 * 
	 * @return energy
	 */
	public double getEnergy() {
		return energy;
	}
	/**
	 * sets the energy of the robot to given amount d
	 * 
	 * @param d
	 * @pre the given amount of energy cannot be negative and cannot be larger than the maximum energy
	 * 		| isValidEnergy(d)
	 * @post the energy of the robot is now the given amount d 
	 * 		| new energy = d
	 */
	public void setEnergy(double d) {
		assert !(d < 0);
		assert !(d > getMaxEnergy());
		energy = d;
	}
	/**
	 * Returns whether the given energy amount is not smaller than 0 and not larger than the maximum energy
	 * @param d
	 * @pre		the given amount cannot be smaller than zero
	 * 			| !(d <0)
	 * @pre 	the given amount cannot be larger than the maximum energy
	 * 			| !(d>getMaxEnergy())
	 * @return
	 */
	private boolean isValidEnergy(double d){
		assert !(d<0);
		assert !(d>getMaxEnergy());
		return true;

	}
	/**
	 * Returns the fraction of energy left from the total amount of energy
	 * possibly available.
	 * 
	 * @pre We cannot divide through 0 
	 * 		| !(getMaxEnergy()==0)
	 * @return getEnergy()/getMaxEnergy()
	 */
	public double getFractionEnergy() {
		assert !(getMaxEnergy() == 0);
		return getEnergy() / getMaxEnergy();
	}
	/**
	 * recharges the robot with the given energy added to it's original energy
	 * 
	 * @pre amount cannot be negative and not larger than the maximum energy 
	 * 		| isValidEnergy(amount)
	 * @pre	the newly set energy level cannot be larger than the maximum energy and cannot be negative
	 * 		| isValidEnergy(amount+getEnergy())
	 * @post energy is now the original energy incremented with the given amount 
	 * 		| new.energy = energy +
	 *       amount
	 */
	public void recharge(int amount) {
		assert isValidEnergy(amount);
		assert isValidEnergy(getEnergy() + amount);
		setEnergy(getEnergy() + amount);
	}
	private double energy;
	/**
	 * returns the energy required to move a robot from 1 position to an
	 * Adjacent position the energy required for movement is 500 watt-second per
	 * move
	 * 
	 * @return MAX_ENERGY
	 */
	public double getMaxEnergy() {
		return MAX_ENERGY;
	}
	private final double MAX_ENERGY = 20000;
	/**
	 * returns the energy required to move a robot from 1 position to an
	 * Adjacent position the energy required for movement is 500 watt-second per
	 * move
	 * 
	 * @return movementEnergy
	 */
	public double getMovementEnergy() {
		return movementEnergy;
	}
	private final int movementEnergy = 500;
	/**
	 * returns the amount of energy required to turn the robot on the board at an
	 * angle of 90 degrees the required energy for a turn is 100 watt-second
	 * 
	 * @return turnEnergy
	 */
	public int getTurnEnergy() {
		return turnEnergy;
	}
	private final int turnEnergy = 100;
	/**
	 * returns the upper x boundary of the board the upper x boundary determines
	 * the width of the board
	 * 
	 * @return UpperXBoundary
	 */
	private long getUpperXBoundary() {
		return upperXBoundary;
	}
	private final long upperXBoundary = Long.MAX_VALUE;
	/**
	 * returns the upper y boundary of the board the upper y boundary determines
	 * the height of the board
	 * 
	 * @return upperYBoundary
	 */
	private long getUpperYBoundary() {
		return upperYBoundary;
	}
	private final long upperYBoundary = Long.MAX_VALUE;
	/**
	 * method that sets the position of the robot to the given position on the board
	 * 
	 * @param position
	 *            the new position of the robot on the board
	 * @post the robots position is the new position given in the argument | new
	 *       this.position = position
	 * @throws IllegalNumberException
	 *             the robot can not have this number as a position | !
	 *             isValidPosition
	 */
	public void setPosition(long[] position) throws IllegalPositionException {
		if ((position[0] > getUpperXBoundary()) || ((position[0] < 0)))
			throw new IllegalPositionException();
		if ((position[1] > getUpperYBoundary()) || ((position[1] < 0)))
			throw new IllegalPositionException();
		this.position = position;
	}
	/**
	 * gives the current position of the robot
	 * 
	 * @return position
	 */
	public long[] getPosition() {
		return position;
	}
	/**
	 * Check whether a position is on the board and thus valid
	 * 
	 * @param position
	 *            the position on the board
	 * @throws IllegalPositionException
	 *             (position.this) this is not a valid position;
	 * @return True if and only if the given position is on the board |
	 *         (!(position[0]<0) && !(position[1]<0) && !(position[0]>
	 *         getMaxEnergy()) && !(position[1]> getMaxEnergy()))
	 */
	public boolean isValidPosition(long position[]) throws IllegalPositionException {
		if (position[0] < 0)
			throw new IllegalPositionException();
		if (position[1] < 0)
			throw new IllegalPositionException();
		if (!(position[0] > getUpperXBoundary()))
			throw new IllegalPositionException();
		if (!(position[1] > getUpperYBoundary()))
			throw new IllegalPositionException();
		return true;
	}
	/**
	 * Moves the robot in the current orientation it is set in
	 * 
	 * @throws IllegalPositionException 
	 * @post moves the robot 1 step forward in the orientation it is currently in
	 * 		| if (getOrientaion()==0) new getPosition()[0] == old getPosition()[0] -1
	 * 		| if (getOrientaion()==1) new getPosition()[1] == old getPosition()[1] -1
	 *  	| if (getOrientaion()==2) new getPosition()[0] == old getPosition()[0] +1
	 *   	| if (getOrientaion()==3) new getPosition()[1] == old getPosition()[1] +1 
	 */
	public void move() throws IllegalPositionException {
		long[] tempposition = getPosition();
		if (getOrientation()==0){
			tempposition[0] -=1;
			setPosition(tempposition);
		}
		if (getOrientation()==1){
			tempposition[1] -=1;
			setPosition(tempposition);
		}
		if (getOrientation()==2){
			tempposition[0] +=1;
			setPosition(tempposition);
		}
		if (getOrientation()==3){
			tempposition[1] +=1;
			setPosition(tempposition);
		}	
		setEnergy(getEnergy() - getMovementEnergy());
	}
	private long[] position;
	/**
	 * gives the current orientation of the robot screenwise 0 = west(left), 1 =
	 * north(up), 2 = east(right), 3 = south(down)
	 * 
	 * @return orientation
	 */
	public int getOrientation() {
		return orientation;
	}
	/**
	 * sets the orientation of the robot on the board screenwise 0 = west(left),
	 * 1 = north(up), 2 = east(right), 3 = south(down)
	 * 
	 * @param i is the orientation given by an integer
	 * @post sets the orientation to a integer closest to the presumed wanted allowed integer if the given integer is negative
	 * 			| new orientation = i+4
	 * @post sets the orientation to a integer closest to the presumed wanted allowed integer if the given integer is larger than 3
	 * 			| new orientation = Math.abs(i)%4
	 * @post sets the new orientation to the given integer if the integer either 0, 1, 2 or 3
	 * 			| new orientation = i
	 */
	public void setOrientation(int i) {
		while (i < 0)
			i += 4;
		orientation = Math.abs(i) % 4;
	}
	/**
	 * Check whether an orientation is an existing one
	 * 
	 * @param orientation
	 *            the orientation of the robot
	 * @return True if and only if the given orientation is left, right, up or
	 *         down | orientation.equals(left || right || up || down)
	 */
	public boolean isValidOrientation(int orientation) {
		if (orientation == 0 || orientation == 1 || orientation == 2 || orientation == 3){
			return true;
		}else{			
		return false;
		}
	}
	/**
	 * Turns the orientation of the robot 90 degrees clockwise (if viewed from top view)
	 * 
	 * @post	turns the robot clockwise
	 * 			| new getOrientation() == getOrientation()+1
	 */
	public void turnClockWise(){
		setOrientation(getOrientation()+1);
		setEnergy(getEnergy() - getTurnEnergy());
	}
	/**
	 * Turns the orientation of the robot 90 degrees counterclockwise (if viewed from top view)
	 * 
	 * @post	turns the robot clockwise
	 * 			| new getOrientation() == getOrientation()-1
	 */
	public void turnCounterClockWise(){
		setOrientation(getOrientation()-1);
		setEnergy(getEnergy() - getTurnEnergy());
	}
	private int orientation;
	/**
	 * Return a binary boolean whether the implementation of GetEnergyRequiredToReach() and MoveNextTo() take into account that turning also requires energy
	 *
	 * @return 1
	 */
	public int isGetEnergyRequiredToReachAndMoveNextTo16Plus(){
		return 1;
	}
	/**
	 * returns energy required to reach a certain position given by x and y

	 * @param 	robot
	 * 			the robot that should move
	 * @param 	x
	 * 			the x coordinate of the destination
	 * @param 	y
	 * 			the y coordinate of the destination
	 * @throws IllegalPositionException 
	 * @pre		robot has to be an existing robot
	 * 			| isValidRobot(robot)
	 * @pre 	x and y should make a valid position
	 * 			| isValidPosition(position)
	 */
	public double getEnergyRequiredToReach(long x, long y) throws IllegalPositionException{
		return getMovementEnergyRequiredToReach(x, y)+getTurnEnergyRequiredTo(x, y);
	}
	private double getMovementEnergyRequiredToReach(long x, long y) throws IllegalPositionException {
		double a =0 ;
		long[] position = null;
		position[0] = x;
		position[1] = y;		
		assert isValidPosition(position);
		a += Math.abs(getPosition()[0] - x);
		a += Math.abs(getPosition()[1] - y);
		return (a-1)*getMovementEnergy();
	}
	private double getTurnEnergyRequiredTo(long x, long y) {
		double b=0;
		if (getPosition()[0]>x){
			switch (getOrientation()){
			case 0:
				b+=0;
			case 1:
				b+=1;
			case 2:
				b+=2;
			case 3:
				b=+1;
			}
		}else if(getPosition()[0]<x){
			switch (getOrientation()){
			case 0:
				b+=2;
			case 1:
				b+=1;
			case 2:
				b+=0;
			case 3:
				b=+1;
			}
		}else if(getPosition()[0] == x){
			b+=0;
		}
		if (getPosition()[1]<y){
			switch (getOrientation()){
			case 0:
				b+=1;
			case 1:
				b+=2;
			case 2:
				b+=1;
			case 3:
				b=+0;
			}			
		}else if (getPosition()[1]>y){
			switch (getOrientation()){
			case 0:
				b+=1;
			case 1:
				b+=0;
			case 2:
				b+=1;
			case 3:
				b=+2;
			}			
		}else if (getPosition()[1]==y){
			b+=0;
		}
		return b*getTurnEnergy();
	}	
	/**
	 * Returns a integer in which quadrant the given robot lies towards the current robot.
	 * if either of the coordinates are the same as the current robot give a special integer
	 * 1 |12|2
	 * -------
	 * 14|0 |23
	 * -------
	 * 4 |34|3
	 * 
	 * @param robot
	 * @return the number of the quadrant
	 */
	private int findQuadrant(IRobot robot) {
		if ((getPosition()[0]>robot.getPosition()[0]) && (getPosition()[1]>robot.getPosition()[1]))
			return 1;
		if ((getPosition()[0]==robot.getPosition()[0]) && (getPosition()[1]>robot.getPosition()[1]))
			return 12;
		if ((getPosition()[0]<robot.getPosition()[0]) && (getPosition()[1]>robot.getPosition()[1]))
			return 2;
		if ((getPosition()[0]<robot.getPosition()[0]) && (getPosition()[1]==robot.getPosition()[1]))
			return 23;
		if ((getPosition()[0]<robot.getPosition()[0]) && (getPosition()[1]<robot.getPosition()[1]))
			return 3;
		if ((getPosition()[0]==robot.getPosition()[0]) && (getPosition()[1]<robot.getPosition()[1]))
			return 34;
		if ((getPosition()[0]>robot.getPosition()[0]) && (getPosition()[1]<robot.getPosition()[1]))
			return 4;
		if ((getPosition()[0]>robot.getPosition()[0]) && (getPosition()[1]==robot.getPosition()[1]))
			return 14;
		if ((getPosition()[0]==robot.getPosition()[0]) && (getPosition()[1]==robot.getPosition()[1])){
			return 0;
		}else{
			return 0;
		}
		
	}
	/**
	 * places the robot next to the given robot, or at least as close as they both can get.
	 * 
	 * @param robot
	 * @throws IllegalPositionException 
	 */
	public void moveNextTo(IRobot robot) throws IllegalPositionException {
		if (getEnergy()<robot.getEnergy()){
			while (isValidEnergy(robot.getEnergy()-getMovementEnergy())){
				moveOnQuadrant(robot);
				if (!isWantedEndPosition(robot))
					moveOnQuadrant(robot);
			}
		}else {
			robot.moveNextTo((IRobot)this);
		}
	}
	/**
	 * checks whether the position of the given robot is next to the position of the current robot.
	 * @param robot
	 * @return true if position of given robot is next to position of the current robot
	 */
	private boolean isWantedEndPosition(IRobot robot) {
		if (getPosition()[0]-1 == robot.getPosition()[0] && getPosition()[1]-1 == robot.getPosition()[1])
			return true;
		if (getPosition()[0] == robot.getPosition()[0] && getPosition()[1]-1 == robot.getPosition()[1])
			return true;
		if (getPosition()[0]+1 == robot.getPosition()[0] && getPosition()[1]-1 == robot.getPosition()[1])
			return true;
		if (getPosition()[0]-1 == robot.getPosition()[0] && getPosition()[1] == robot.getPosition()[1])
			return true;
		if (getPosition()[0]+1 == robot.getPosition()[0] && getPosition()[1] == robot.getPosition()[1])
			return true;
		if (getPosition()[0]-1 == robot.getPosition()[0] && getPosition()[1]+1 == robot.getPosition()[1])
			return true;
		if (getPosition()[0] == robot.getPosition()[0] && getPosition()[1]+1 == robot.getPosition()[1])
			return true;
		if (getPosition()[0]+1 == robot.getPosition()[0] && getPosition()[1]+1 == robot.getPosition()[1]){
			return true;
		}else{
		return false;
		}
	}
	/**
	 * moves the given robot closer to the current robot based on it's location(quadrant)
	 * @param robot
	 * @throws IllegalPositionException
	 * @post if the location of the given robot is left of the current robot, bring it to the same horizontal position on the board if it's not already on the same vertical level as the current robot.
	 * 			| new robot.getPosition()[0] = getPosition()[0]-1
	 * @post if the location of the given robot is right of the current robot, bring it to the same horizontal position on the board if it's not already on the same vertical level as the current robot.
	 * 			| new robot.getPosition()[0] = getPosition()[0]+1
	 * @post if the location of the given robot is on the same vertical level left of the current robot, move it horizontal until it is next to the current robot.
	 * 			| new robot.getPosition()[0] = getPosition()[0]-1
	 * @post if the location of the given robot is on the same vertical level right of the current robot, move it horizontal until it is next to the current robot.
	 * 			| new robot.getPosition()[0] = getPosition()[0]+1
	 * @post if the location of the given robot is on the same horizontal level above as the current robot, move it vertical until it is next to the current robot.
	 * 			| new robot.getPosition()[1] = getPosition()[1]-1
	 * @post if the location of the given robot is on the same horizontal level under as the current robot, move it vertical until it is next to the current robot.
	 * 			| new robot.getPosition()[1] = getPosition()[1]+1
	 */
	private void moveOnQuadrant(IRobot robot) throws IllegalPositionException {
		switch (findQuadrant(robot)){
		case 1:
			if (robot.getOrientation() == 1 || robot.getOrientation() == 0){
				while (robot.getOrientation()!= 2 && isValidEnergy(robot.getEnergy()-getTurnEnergy())){
					robot.turnClockWise();
				}
			} else if (robot.getOrientation() == 3 && isValidEnergy(robot.getEnergy()-getTurnEnergy())){
				robot.turnCounterClockWise();
			} 
			while ((getPosition()[0]>robot.getPosition()[0]) && isValidEnergy(robot.getEnergy()-getMovementEnergy())){
				robot.move();
			}
			break;
		case 12:
			if (robot.getOrientation() == 1 || robot.getOrientation() == 2){
				while (robot.getOrientation()!= 3 && isValidEnergy(robot.getEnergy()-getTurnEnergy())){
					robot.turnClockWise();
				}
			} else if (robot.getOrientation() == 0 && isValidEnergy(robot.getEnergy()-getTurnEnergy())){
				robot.turnCounterClockWise();
			} 
			while ((getPosition()[1]-1>robot.getPosition()[1]) && isValidEnergy(robot.getEnergy()-getMovementEnergy())){
				robot.move();
			}
			break;
		case 2:
			if (robot.getOrientation() == 3 || robot.getOrientation() == 2){
				while (robot.getOrientation()!= 0 && isValidEnergy(robot.getEnergy()-getTurnEnergy())){
					robot.turnClockWise();
				}
			} else if (robot.getOrientation() == 1 && isValidEnergy(robot.getEnergy()-getTurnEnergy())){
				robot.turnCounterClockWise();
			} 
			while ((getPosition()[0]<robot.getPosition()[0]) && isValidEnergy(robot.getEnergy()-getMovementEnergy())){
				robot.move();
			}
			break;
		case 23:
			if (robot.getOrientation() == 3 || robot.getOrientation() == 2){
				while (robot.getOrientation()!= 0 && isValidEnergy(robot.getEnergy()-getTurnEnergy())){
					robot.turnClockWise();
				}
			} else if (robot.getOrientation() == 1 && isValidEnergy(robot.getEnergy()-getTurnEnergy())){
				robot.turnCounterClockWise();
			} 
			while ((getPosition()[0]+1<robot.getPosition()[0]) && isValidEnergy(robot.getEnergy()-getMovementEnergy())){
				robot.move();
			}
			break;
		case 3:
			if (robot.getOrientation() == 3 || robot.getOrientation() == 2){
				while (robot.getOrientation()!= 0 && isValidEnergy(robot.getEnergy()-getTurnEnergy())){
					robot.turnClockWise();
				}
			} else if (robot.getOrientation() == 1 && isValidEnergy(robot.getEnergy()-getTurnEnergy())){
				robot.turnCounterClockWise();
			} 
			while ((getPosition()[0]<robot.getPosition()[0]) && isValidEnergy(robot.getEnergy()-getMovementEnergy())){
				robot.move();
			}
			break;
		case 34:
			if (robot.getOrientation() == 3 || robot.getOrientation() == 0){
				while (robot.getOrientation()!= 1 && isValidEnergy(robot.getEnergy()-getTurnEnergy())){
					robot.turnClockWise();
				}
			} else if (robot.getOrientation() == 2 && isValidEnergy(robot.getEnergy()-getTurnEnergy())){
				robot.turnCounterClockWise();
			} 
			while ((getPosition()[1]+1<robot.getPosition()[1]) && isValidEnergy(robot.getEnergy()-getMovementEnergy())){
				robot.move();
			}
			break;
		case 4:
			if (robot.getOrientation() == 1 || robot.getOrientation() == 0){
				while (robot.getOrientation()!= 2 && isValidEnergy(robot.getEnergy()-getTurnEnergy())){
					robot.turnClockWise();
				}
			} else if (robot.getOrientation() == 3 && isValidEnergy(robot.getEnergy()-getTurnEnergy())){
				robot.turnCounterClockWise();
			} 
			while ((getPosition()[0]>robot.getPosition()[0]) && isValidEnergy(robot.getEnergy()-getMovementEnergy())){
				robot.move();
			}
			break;
		case 14:
		if (robot.getOrientation() == 1 || robot.getOrientation() == 0){
			while (robot.getOrientation()!= 2 && isValidEnergy(robot.getEnergy()-getTurnEnergy())){
				robot.turnClockWise();
			}
		} else if (robot.getOrientation() == 3 && isValidEnergy(robot.getEnergy()-getTurnEnergy())){
			robot.turnCounterClockWise();
		} 
		while ((getPosition()[0]-1>robot.getPosition()[0]) && isValidEnergy(robot.getEnergy()-getMovementEnergy())){
			robot.move();
		}
		break;
		case 0:
			if (isValidEnergy(robot.getEnergy()-getMovementEnergy()))
				move();
			break;
		}
	}
}
